#include "PnPSolver.h"
#include "utils.h"
#include <iostream>
#include <random>

int main() {
    // 相机内参（以TUM数据集为例）
    // double fx = 520.9, fy = 521.0, cx = 325.1, cy = 249.7;
    
    // // 生成测试数据
    // std::vector<Eigen::Vector3d> landmarks;
    // std::vector<Eigen::Vector2d> bearings;
    // Eigen::Vector3d true_translation;
    // Eigen::Quaterniond true_rotation;
    
    // GenerateTestData(landmarks, bearings, true_translation, true_rotation, fx, fy, cx, cy);
    
    // // 添加噪声到初始估计
    // std::random_device rd;
    // std::mt19937 gen(rd());
    // std::normal_distribution<double> trans_noise(0.0, 0.1);
    // std::normal_distribution<double> rot_noise(0.0, 0.05);
    
    // Eigen::Vector3d initial_translation = true_translation;
    // initial_translation[0] += trans_noise(gen);
    // initial_translation[1] += trans_noise(gen);
    // initial_translation[2] += trans_noise(gen);
    
    // Eigen::Quaterniond initial_rotation = true_rotation;
    // Eigen::Vector3d euler_angles = initial_rotation.toRotationMatrix().eulerAngles(0, 1, 2);
    // euler_angles[0] += rot_noise(gen);
    // euler_angles[1] += rot_noise(gen);
    // euler_angles[2] += rot_noise(gen);
    // initial_rotation = Eigen::AngleAxisd(euler_angles[0], Eigen::Vector3d::UnitX())
    //                  * Eigen::AngleAxisd(euler_angles[1], Eigen::Vector3d::UnitY())
    //                  * Eigen::AngleAxisd(euler_angles[2], Eigen::Vector3d::UnitZ());
    
    // std::cout << "True translation: " << true_translation.transpose() << std::endl;
    // std::cout << "Initial translation: " << initial_translation.transpose() << std::endl;
    // std::cout << "True rotation: " << true_rotation.coeffs().transpose() << std::endl;
    // std::cout << "Initial rotation: " << initial_rotation.coeffs().transpose() << std::endl;
    
    // // 创建求解器并求解
    // PnPSolver solver(fx, fy, cx, cy);
    // bool success = solver.Solve(landmarks, bearings, initial_translation, initial_rotation);
    
    // if (success) {
    //     std::cout << "\n=== Optimization Results ===" << std::endl;
    //     std::cout << "Final translation error: " << (initial_translation - true_translation).norm() << std::endl;
        
    //     // 计算旋转误差（角度）
    //     Eigen::Quaterniond q_error = true_rotation.conjugate() * initial_rotation;
    //     double angle_error = 2.0 * std::acos(std::abs(q_error.w())) * 180.0 / M_PI;
    //     std::cout << "Final rotation error: " << angle_error << " degrees" << std::endl;
    // }
    
    return 0;
}